#include "workthread/workthread.h"

#include <vector>
#include <string>
#include <sstream>
#include <stdio.h>
#include <fstream>
#include <QFile>
#include <QFileInfo>

#include <pcl/console/parse.h>
#include <vtkPlaneSource.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>


#include "rapter/typedefs.h"                    // _2d::, _3d::, Scalar, PointPrimitiveT, PointContainerT
#include "rapter/optimization/segmentation.h"   // segmentCli, orientPoints, patchify

#include "rapter/optimization/candidateGenerator.h"
#include "rapter/optimization/problemSetup.h"
#include "rapter/optimization/solver.h"
#include "rapter/optimization/represent.hpp"
#include "rapter/optimization/merging.h"


#include "rapter/visualization/impl/visualization.hpp"
#include "rapter/typedefs.h"
#include "rapter/primitives/impl/planePrimitive.hpp"
#include "rapter/primitives/impl/linePrimitive.hpp"
#include "pcl/visualization/pcl_visualizer.h"
#include "rapter/processing/impl/angleUtil.hpp" // appendAngles...
#include "rapter/primitives/pointPrimitive.h"

workThread::workThread(
        /************************General**********************/
        std::string cloudPath,
        int         N,
        float       myScale,                     //    = 0.02;
        float       myAngleLimit,                //    = 10.;
        float       myPopLimit,                  //    = 10.;
        bool        verbose,                     //    = false;
        bool        no_paral,                    //    = false;
        /****************generate*****************************/
        float       mySmallThreshMult,           //    = 4.;
        float       myAngleLimitDivisor,         //    = 1.0;
        float       patch_dist_limit,            //    = 1.0;
        bool        safe_mode,                   //    = false;
        bool        keepSingles,                 //   = true;
        bool        allowPromoted,               //   = true;
        bool        adoptChanged,                //   = false;
        bool        myTripletSafe,               //    = true;
        /****************Formulate*****************************/
        float       myData,                      //    = 1e5;
        float       myPw,                        //    = 0.01;
        float       collapseThreshDeg,           //    = 0.4;
        /****************Merge*****************************/
        int         adopt                        //   = 0;
        )
{
    this->cloudPath = cloudPath;
    this->N = N;
    this->myScale = myScale;
    this->myAngleLimit = myAngleLimit;
    this->myPopLimit = myPopLimit;
    this->verbose = verbose;
    this->no_paral = no_paral;
    this->mySmallThreshMult = mySmallThreshMult;
    this->myAngleLimitDivisor = myAngleLimitDivisor;
    this->patch_dist_limit = patch_dist_limit;
    this->safe_mode = safe_mode;
    this->keepSingles = keepSingles;
    this->allowPromoted = allowPromoted;
    this->adoptChanged = adoptChanged;
    this->myTripletSafe = myTripletSafe;
    this->myData = myData;
    this->myPw = myPw;
    this->collapseThreshDeg = collapseThreshDeg;
    this->adopt = adopt;

}

void workThread::run()
{
   this->work(cloudPath, N,myScale,myAngleLimit,myPopLimit,verbose, no_paral,
              mySmallThreshMult,myAngleLimitDivisor,patch_dist_limit,safe_mode,keepSingles,allowPromoted,adoptChanged,myTripletSafe,
              myData, myPw,collapseThreshDeg,
              adopt);
}

void workThread::work(
        /************************General**********************/
        std::string cloudPath,
        int         N,
        float       myScale,                     //    = 0.02;
        float       myAngleLimit,                //    = 10.;
        float       myPopLimit,                  //    = 10.;
        bool        verbose,                     //    = false;
        bool        no_paral,                    //    = false;
        /****************generate*****************************/
        float       mySmallThreshMult,           //    = 4.;
        float       myAngleLimitDivisor,         //    = 1.0;
        float       patch_dist_limit,            //    = 1.0;
        bool        safe_mode,                   //    = false;
        bool        keepSingles,                 //   = true;
        bool        allowPromoted,               //   = true;
        bool        adoptChanged,                //   = false;
        bool        myTripletSafe,               //    = true;
        /****************Formulate*****************************/
        float       myData,                      //    = 1e5;
        float       myPw,                        //    = 0.01;
        float       collapseThreshDeg,           //    = 0.4;
        /****************Merge*****************************/
        int         adopt                        //   = 0;
        )
{
    using namespace rapter::vis;
    std::string prefix=cloudPath.substr(0,cloudPath.find_last_of("/")+1);
    rapter::AnglesT angle_gens ({Scalar(0.)});      //TODO
    rapter::AnglesT candAngleGens ({Scalar(0.)});   //TODO
    std::string primitives                      = "patches.csv";
    std::string associations                    = "points_primitives.csv";
    float       smallThreshDiv                  = 2.;
    float       smallThreshLimit                = 0.;
    int         promRem                         = 0;
    int         iteration                       = 0;
    bool        decreaseLevel                   = false;
    float       myVariableLimit                 = 1000.;
    float       mySpatial                       = myPw / 10;
    rapter::AnglesT myAnglegens ({Scalar(0.),Scalar(90.)});  //TODO
    int         useAllGens                      = std::min(5 , N - 1);

    std::fstream out;


    while (iteration <= N)
    {
        if(decreaseLevel)
        {
             mySmallThreshMult = float(int(mySmallThreshMult /smallThreshDiv ) );
        }
        if(mySmallThreshMult <= smallThreshLimit)
        {
            mySmallThreshMult = int(smallThreshLimit);
            if(decreaseLevel)
            {
                adopt = 1;
                if(!adoptChanged)
                {
                    adoptChanged = true;
                    useAllGens   = iteration + 2;
                    N = std::max(N , (int)useAllGens + 3);
                }
            }
        }

        decreaseLevel = true;
        std::cout<<"smallThreshMult:"<<mySmallThreshMult<<std::endl;
        std::cout<<"__________________________________________________________"<<std::endl;
        std::cout<<"Start iteration:"<<iteration<<std::endl;

        out.open(prefix + "log",ios::app);
        out<<"smallThreshMult:"<<mySmallThreshMult<<"\n";
        out<<"__________________________________________________________\n";
        out<<"Start iteration:"<<iteration<<"\n";
        out.close();


        int prevId = iteration - 1;

        char * tmp = new char[255];
        char * tmp1 = new char[255];
        if(iteration >0)
        {
            sprintf(tmp,"primitives_merged_it%d.csv",prevId);
            primitives = tmp;
            sprintf(tmp,"points_primitives_it%d.csv",prevId);
            associations = tmp;
        }

        promRem = rapter::CandidateGenerator::myGenerateCli< rapter::_3d::PrimitiveContainerT
                                                           , rapter::PointContainerT
                                                           , rapter::Scalar
                                                           , rapter::PointPrimitiveT
                                                           , rapter::_3d::PrimitiveT
                                                           >(
                    cloudPath,                                                              //-cloud
                    rapter::Scalar(myScale),                                                    //-sc
                    prefix + primitives,                                                        //-p -prims
                    mySmallThreshMult,                                                          //--small-thresh-mult
                    prefix + associations,                                                      //-a -assoc
                    rapter::Scalar(myAngleLimit),                                               //-al
                    myAngleLimitDivisor,                                                        //-ald
                    rapter::Scalar(patch_dist_limit),                                           //--patch-dist-limit
                    angle_gens,                                                                 //-amgle-gen
                    myPopLimit,                                                                 //--patch-pop-limit
                    safe_mode,                                                                  //safe-mode
                    myVariableLimit,                                                            //--var-limit
                    keepSingles,                                                                //keepSingles
                    allowPromoted,                                                              //allowPromoted
                    myTripletSafe,                                                              //tripletSafe
                    verbose,                                                                    //verbose
                    no_paral                                                                    //no_paral
                    );
        std::cout<<"[rapter.py] Remaining smalls to promote: "<<promRem<<std::endl;
        out<<"[rapter.py] Remaining smalls to promote: "<<promRem<<"\n";

        if(promRem != 0)
            decreaseLevel = false;

        sprintf(tmp,"candidates_it%d.csv",iteration);
        std::string candidates_it = tmp;
        rapter::ProblemSetup::myFormulateCli< rapter::_3d::PrimitiveContainerT
                                                      , rapter::PointContainerT
                                                      , rapter::_3d::PrimitiveT
                                                      , rapter::PointPrimitiveT
                                                      , rapter::_3d::MyFinitePlaneToFinitePlaneCompatFunctor
                                                      >(
                    cloudPath,                                                              //cloud_path
                    prefix + candidates_it,                                                     //candidates_path
                    prefix + associations,                                                      //assoc_path
                    verbose,                                                                    //verbose
                    myScale,                                                                    //-scale
                    myData,                                                                     //unary
                    myPw,                                                                       //pw
                    0.,                                                                         //cmp
                    "spatsqrt",                                                                 //cost_string               --cost-fn
                    123456,                                                                     //srand_val                 --srand
                    "problem",                                                                  //problem_rel_path          --rod
                    angle_gens,                                                                 //angle_gens                --angle-gen
                    0,                                                                          //dir_id_bias
                    "assoc",                                                                    //data_cost_mode_str
                    "patch",                                                                    //constr_mode_str           #see the code for more information
                    myPopLimit,                                                                 //patch_population          --patch-pop-limit
                    collapseThreshDeg,                                                          //collapseAngleDeg          --collapse-angle-deg
                    prefix + "energy1.csv",                                                      //energy_path
                    0.,                                                                         //freq_weight
                    0,                                                                          //--no-clusters
                    mySpatial,                                                                  //spatial_weight_coeff      --spat-weight   pw/10
                    2.,                                                                         //spatial_weight_dist_mult  --spat-dist-mult
                    false,                                                                      //useAngleGen   ????            --use-angle-gen
                    myAngleLimit,                                                               //trunAngle                 --trunc-angle
                    no_paral                                                                    //no_paral
                    );


        rapter::Solver::mySolve< rapter::_3d::PrimitiveContainerT
                                         , rapter::_3d::InnerPrimitiveContainerT
                                         , rapter::_3d::PrimitiveT
                                         >(
                    true,                                           //verbose
                    "bonmin",                                       //solver_str
                    prefix + "problem",                             //project_path
                    -1,                                             //max_time
                    0,                                              //bmode 0 for bonmin
                    ".",                                            //rel_out_path
                    "",                                             //x0_path --x0
                    prefix + candidates_it                          //candidates_path
                    );


        if(iteration == useAllGens)
        {
            angle_gens = myAnglegens;
            candAngleGens = angle_gens;
        }

        //*********************************************RunRepr*****************************************
        sprintf(tmp,"primitives_it%d.bonmin.csv",iteration);
        std::string rprPrims     = tmp;
        std::string rprAssoc     = "points_primitives.csv";
        sprintf(tmp,"representatives_it%d.csv",iteration);
        std::string rprRepr      = tmp;
        sprintf(tmp,"points_representatives_it%d.csv",iteration);
        std::string rprReprAssoc = tmp;
        sprintf(tmp,"candidates_representatives_it%d.csv",iteration);
        std::string rprCands     = tmp;
        sprintf(tmp,"representatives_it%d.bonmin.csv",iteration);
        std::string rprReprOpt   = tmp;
        sprintf(tmp,"primitives_it%d.bonmin.lvl1.csv",iteration);
        std::string rprPrimBak   = tmp;

        rapter::myRepresentCli< rapter::_3d::PrimitiveContainerT
                         , rapter::PointContainerT
                         , rapter::_3d::PrimitiveT
                         , rapter::PointPrimitiveT
                         , rapter::_3d::MyFinitePlaneToFinitePlaneCompatFunctor
                         >(
                    cloudPath,
                    prefix + rprPrims,
                    prefix + rprAssoc,
                    myScale,
                    angle_gens
                    );

        QFile::rename(QString::fromStdString(prefix + "representatives.csv") ,
                      QString::fromStdString(prefix + rprRepr));
        QFile::rename(QString::fromStdString(prefix + "points_representatives.csv") ,
                      QString::fromStdString(prefix + rprReprAssoc));
        sprintf(tmp,"candidates_it%d.csv",iteration + 1);
        QFileInfo info1(QString::fromStdString(prefix + tmp));
        if(info1.exists())
        {
            sprintf(tmp1,"candidates_it%d_tmp.csv",iteration + 1);
            QFile::rename(QString::fromStdString(prefix + tmp) ,
                          QString::fromStdString(prefix + tmp1));
        }
        rapter::CandidateGenerator::myGenerateCli< rapter::_3d::PrimitiveContainerT
                                                           , rapter::PointContainerT
                                                           , rapter::Scalar
                                                           , rapter::PointPrimitiveT
                                                           , rapter::_3d::PrimitiveT
                                                           >(
                    cloudPath,                                                              //-cloud
                    rapter::Scalar(myScale),                                                    //-sc
                    prefix + rprRepr,                                                           //-p -prims
                    mySmallThreshMult,                                                          //--small-thresh-mult
                    prefix + rprReprAssoc,                                                      //-a -assoc
                    rapter::Scalar(myAngleLimit),                                               //-al
                    myAngleLimitDivisor,                                                        //-ald
                    rapter::Scalar(patch_dist_limit),                                           //--patch-dist-limit
                    angle_gens,                                                                 //-amgle-gen
                    myPopLimit,                                                                 //--patch-pop-limit
                    safe_mode,                                                                  //safe-mode
                    myVariableLimit,                                                            //--var-limit
                    keepSingles,                                                                //keepSingles
                    allowPromoted,                                                              //allowPromoted
                    myTripletSafe,                                                              //tripletSafe
                    verbose,                                                                    //verbose
                    no_paral                                                                    //no_paral
                    );

        sprintf(tmp,"candidates_it%d.csv",iteration + 1);
        QFile::rename(QString::fromStdString(prefix + tmp) ,
                      QString::fromStdString(prefix + rprCands));
        sprintf(tmp,"candidates_it%d_tmp.csv",iteration + 1);
        QFileInfo info2(QString::fromStdString(prefix + tmp));
        if(info2.exists())
        {
            sprintf(tmp1,"candidates_it%d.csv",iteration + 1);
            QFile::rename(QString::fromStdString(prefix + tmp) ,
                          QString::fromStdString(prefix + tmp1));
        }

        rapter::ProblemSetup::myFormulateCli< rapter::_3d::PrimitiveContainerT
                                                      , rapter::PointContainerT
                                                      , rapter::_3d::PrimitiveT
                                                      , rapter::PointPrimitiveT
                                                      , rapter::_3d::MyFinitePlaneToFinitePlaneCompatFunctor
                                                      >(
                    cloudPath,                                                              //cloud_path
                    prefix + rprCands,                                                          //candidates_path
                    prefix + rprReprAssoc,                                                      //assoc_path
                    verbose,                                                                    //verbose
                    myScale,                                                                    //-scale
                    myData,                                                                     //unary
                    myPw,                                                                       //pw
                    0.,                                                                         //cmp
                    "spatsqrt",                                                                 //cost_string               --cost-fn
                    123456,                                                                     //srand_val                 --srand
                    "problem",                                                                  //problem_rel_path          --rod
                    angle_gens,                                                                 //angle_gens                --angle-gen
                    0,                                                                          //dir_id_bias
                    "assoc",                                                                    //data_cost_mode_str
                    "patch",                                                                    //constr_mode_str           #see the code for more information
                    myPopLimit,                                                                 //patch_population          --patch-pop-limit
                    collapseThreshDeg,                                                          //collapseAngleDeg          --collapse-angle-deg
                    prefix + "energy.csv",                                                      //energy_path
                    0.,                                                                         //freq_weight
                    0,                                                                          //--no-clusters
                    mySpatial,                                                                  //spatial_weight_coeff      --spat-weight   pw/10
                    2.,                                                                         //spatial_weight_dist_mult  --spat-dist-mult
                    false,                                                                      //useAngleGen   ????            --use-angle-gen
                    myAngleLimit,                                                               //trunAngle                 --trunc-angle
                    no_paral                                                                    //no_paral
                    );

        sprintf(tmp,"diag_it%d.gv",iteration);
        std::string rprDiagF    = tmp;
        std::string rprDiagFTmp = rprDiagF + "RprTmp";
        sprintf(tmp,"primitives_it%d.bonmin.csv",iteration);
        sprintf(tmp1,"primitives_it%d_rprtmp.csv",iteration);
        QFile::copy(QString::fromStdString(prefix + tmp),
                    QString::fromStdString(prefix + tmp1));
        QFileInfo info3(QString::fromStdString(prefix + rprDiagF));
        if(info3.exists())
        {
            QFile::rename(QString::fromStdString(prefix + rprDiagF) ,
                          QString::fromStdString(prefix + rprDiagFTmp));
        }
        rapter::Solver::mySolve< rapter::_3d::PrimitiveContainerT
                                         , rapter::_3d::InnerPrimitiveContainerT
                                         , rapter::_3d::PrimitiveT
                                         >(
                    true,                                           //verbose
                    "bonmin",                                       //solver_str
                    prefix + "problem",                             //project_path
                    Scalar(1e10),                                    //
                    0,                                              //bmode 0 for bonmin
                    ".",                                            //rel_out_path
                    "",                                             //x0_path --x0
                    prefix + rprCands                               //candidates_path
                    );
        sprintf(tmp,"primitives_it%d.bonmin.csv",iteration);
        QFile::remove(QString::fromStdString(prefix + rprReprOpt));
        QFile::copy(QString::fromStdString(prefix + tmp),
                    QString::fromStdString(prefix + rprReprOpt));
        QFile::remove( QString::fromStdString(prefix + tmp));
        sprintf(tmp1,"primitives_it%d_rprtmp.csv",iteration);
        QFile::copy(QString::fromStdString(prefix + tmp1),
                    QString::fromStdString(prefix + tmp));
        sprintf(tmp,"diag_it%d.lvl2.gv",iteration);
        QFile::rename(QString::fromStdString(prefix + rprDiagF),
                    QString::fromStdString(prefix + tmp));
        QFileInfo info4(QString::fromStdString(prefix + rprDiagFTmp));
        if(info4.exists())
        {
            QFile::rename(QString::fromStdString(prefix + rprDiagFTmp),
                        QString::fromStdString(prefix + rprDiagF));
        }
        rapter::representBackCli< rapter::_3d::PrimitiveContainerT
                         , rapter::PointContainerT
                         , rapter::_3d::PrimitiveT
                         , rapter::PointPrimitiveT
                         , rapter::_3d::MyFinitePlaneToFinitePlaneCompatFunctor
                         >(
                    cloudPath,
                    prefix + rprPrims,
                    prefix + rprAssoc,
                    prefix + rprReprOpt,
                    myScale,
                    angle_gens
                    );
        QFile::rename(QString::fromStdString(prefix + rprPrims),
                    QString::fromStdString(prefix + rprPrimBak));
        QFile::rename(QString::fromStdString(prefix + "subs.csv"),
                    QString::fromStdString(prefix + rprPrims));

        sprintf(tmp,"primitives_it%d.bonmin.csv",iteration);
        rapter::Merging::myMergeCli< rapter::_3d::PrimitiveContainerT
                                             , rapter::PointContainerT
                                             , rapter::Scalar
                                             , rapter::PointPrimitiveT
                                             , rapter::_3d::PrimitiveT
                                             >(
                    cloudPath,                                                              //cloud_path
                    prefix + tmp,                                                               //prims_path
                    prefix + "points_primitives.csv",                                           //assoc_path
                    angle_gens,                                                                //angle_gens
                    0,                                                                          //size_limit
                    adopt,                                                                          //adopt
                    myPopLimit,                                                                        //--patch-pop-limit
                    no_paral,                                                                      //no-paral
                    2.5,                                                                        //spatial_threshold_mult
                    myScale                                                                        //scale
                    );
        //send Signal
        emit sendIt(iteration);

        if (iteration == 3)
                allowPromoted = false;
        if (iteration == 1)
                keepSingles = false;
        if (iteration == N && promRem != 0)
                N += 1;
        iteration += 1;
    }

}
